// Include files.
#include "EntechFpk.hpp"
#include "DriveDistanceTestController.hpp"

DriveDistanceTestController::DriveDistanceTestController(MecanumDriveSubsystem &drive, HumanDigitalInput &input) :
	  m_asyncResult()
	, m_drive(drive)
	, m_input(input)
	, m_inputReleasedPrevious(true)
{
}

void DriveDistanceTestController::Periodic()
{
	if (m_asyncResult.get() != NULL)
	{
		if (m_asyncResult->GetIsFinished())
		{
			m_asyncResult.reset();
		}
	}
	
	if (m_input.Get() && m_inputReleasedPrevious)
	{
		m_asyncResult = m_drive.DriveDistanceByCartesian(0, 2, 0);
		m_inputReleasedPrevious = false;
	}
	else if (!m_input.Get() && !m_inputReleasedPrevious)
	{
		m_inputReleasedPrevious = true;
	}
}
